#include "cl_process.hpp"

    // OpenCL相关成员
    cl_context context_ = nullptr;
    cl_command_queue command_queue_ = nullptr;
    cl_program program_ = nullptr;
    cl_kernel kernel_ = nullptr;
    cl_mem d_input_ = nullptr;
    cl_mem d_output_ = nullptr;
    cl_mem d_rotation_ = nullptr;
    cl_mem d_translation_ = nullptr;
    #define ANGLE_FILTER_ENABLE 1

 // 初始化OpenCL
    void init_opencl()
    {
        cl_int err;
        
        // 获取平台
        cl_platform_id platform;
        err = clGetPlatformIDs(1, &platform, NULL);
        check_opencl_error(err, "Failed to get platform");
        
        // 获取设备
        cl_device_id device;
        err = clGetDeviceIDs(platform, CL_DEVICE_TYPE_GPU, 1, &device, NULL);
        if (err != CL_SUCCESS) {
          //  RCLCPP_WARN(this->get_logger(), "No GPU found, using CPU");
            err = clGetDeviceIDs(platform, CL_DEVICE_TYPE_CPU, 1, &device, NULL);
            check_opencl_error(err, "Failed to get device");
        }
        
        // 创建上下文
        context_ = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
        check_opencl_error(err, "Failed to create context");
        
        // 创建命令队列
        command_queue_ = clCreateCommandQueue(context_, device, 0, &err);
        check_opencl_error(err, "Failed to create command queue");
        
        // 定义OpenCL kernel
        const char* kernel_source = R"(
            __kernel void transform_cloud(
                __global const float* input,
                __global float* output,
                __constant float* rotation,
                __constant float* translation,
                int num_points)
            {
                int i = get_global_id(0);
                if (i >= num_points) return;
                float baa=0.0;
                int idx = i * 4;
                int idn =(i-1)*4;
                float x = input[idx];
                float y = input[idx + 1];
                float z = input[idx + 2];
                ///加入滤波 计算顺序两点间距离 sin(1)= 0.017452
                
                if(i==0)
                {
                  output[idx+3] = 0;  
                }
                else
                {

                float disx= input[idx]-input[idn];
                float disy= input[idx+1]-input[idn+1];
                float disz= input[idx+2]-input[idn+2];
                float3 dist0 =(float3)(disx,disy,disz);
                float3 dist1 = (float3)(input[idx],input[idx+1],input[idx+2]);
                float3 dist2 = (float3)(input[idn],input[idn+1],input[idn+2]);
                float tsin0=length(dist0);
                float tm1=length(dist1);
                float tm2=length(dist2);
                float tsin1 = tm1*0.05052;
                 if(tsin0>tsin1)
                   {
                       output[idn+3] = -1.0;
                       output[idx+3] = -1.0;


                    }
                   else output[idx+3] =1.0;
                
                }
                
                //滤波结束

                // 旋转
                float x_rot = rotation[0] * x + rotation[1] * y + rotation[2] * z;
                float y_rot = rotation[3] * x + rotation[4] * y + rotation[5] * z;
                float z_rot = rotation[6] * x + rotation[7] * y + rotation[8] * z;
                
                // 平移
                output[idx] = x_rot + translation[0];
                output[idx + 1] = y_rot + translation[1];
                output[idx + 2] = z_rot + translation[2];
              //  output[idx + 3] = input[idx+3];
            }
         __kernel void filter_point(
                __global const float* input,
                __global float* output)
                {
                }
        )";
        
        // 创建程序
        program_ = clCreateProgramWithSource(context_, 1, &kernel_source, NULL, &err);
        check_opencl_error(err, "Failed to create program");
        
        // 编译程序
        err = clBuildProgram(program_, 1, &device, NULL, NULL, NULL);
        if (err != CL_SUCCESS) {
            char log[10240];
            clGetProgramBuildInfo(program_, device, CL_PROGRAM_BUILD_LOG, sizeof(log), log, NULL);
           // RCLCPP_ERROR(this->get_logger(), "OpenCL build error:\n%s", log);
            check_opencl_error(err, "Failed to build program");
        }
        
        // 创建内核
        kernel_ = clCreateKernel(program_, "transform_cloud", &err);
        check_opencl_error(err, "Failed to create kernel");
        
        // 创建旋转和平移矩阵的设备内存
        d_rotation_ = clCreateBuffer(context_, CL_MEM_READ_ONLY, 9 * sizeof(float), NULL, &err);
        check_opencl_error(err, "Failed to create rotation buffer");
        
        d_translation_ = clCreateBuffer(context_, CL_MEM_READ_ONLY, 3 * sizeof(float), NULL, &err);
        check_opencl_error(err, "Failed to create translation buffer");
    }
    
    // 检查OpenCL错误
    void check_opencl_error(cl_int err, const std::string& msg)
    {
        if (err != CL_SUCCESS) {
            // RCLCPP_FATAL(this->get_logger(), "%s (error code: %d)", msg.c_str(), err);
            // rclcpp::shutdown();
        }
    }

     // 使用OpenCL变换点云
    void transform_pointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, 
                             const float* rotation, float x0, float y0, float z0)
    {
        int num_points = cloud->size();
        if (num_points == 0) return;
        
        cl_int err;
        
        // 准备输入数据
        std::vector<float> input_data(num_points * 4);
        for (int i = 0; i < num_points; i++) {
            input_data[i * 4] = cloud->points[i].x;
            input_data[i * 4 + 1] = cloud->points[i].y;
            input_data[i * 4 + 2] = cloud->points[i].z;
            input_data[i * 4 + 3] =0;
        }
        
        // 创建输入输出缓冲区
        if (d_input_) clReleaseMemObject(d_input_);
        d_input_ = clCreateBuffer(context_, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
                                 input_data.size() * sizeof(float), input_data.data(), &err);
        check_opencl_error(err, "Failed to create input buffer");
        
        if (d_output_) clReleaseMemObject(d_output_);
        d_output_ = clCreateBuffer(context_, CL_MEM_WRITE_ONLY,
                                  input_data.size() * sizeof(float), NULL, &err);
        check_opencl_error(err, "Failed to create output buffer");
        
        // 复制旋转矩阵到设备
        err = clEnqueueWriteBuffer(command_queue_, d_rotation_, CL_TRUE, 0,
                                  9 * sizeof(float), rotation, 0, NULL, NULL);
        check_opencl_error(err, "Failed to write rotation matrix");
        
        // 准备平移向量
        float translation[3] = {static_cast<float>(x0), 
                                static_cast<float>(y0), 
                                static_cast<float>(z0)};
        
        // 复制平移向量到设备
        err = clEnqueueWriteBuffer(command_queue_, d_translation_, CL_TRUE, 0,
                                  3 * sizeof(float), translation, 0, NULL, NULL);
        check_opencl_error(err, "Failed to write translation vector");
        
        // 设置内核参数
        err = clSetKernelArg(kernel_, 0, sizeof(cl_mem), &d_input_);
        err |= clSetKernelArg(kernel_, 1, sizeof(cl_mem), &d_output_);
        err |= clSetKernelArg(kernel_, 2, sizeof(cl_mem), &d_rotation_);
        err |= clSetKernelArg(kernel_, 3, sizeof(cl_mem), &d_translation_);
        err |= clSetKernelArg(kernel_, 4, sizeof(int), &num_points);
        check_opencl_error(err, "Failed to set kernel arguments");
        
        // 执行内核
        size_t local_work_size = 128;  // 可以根据设备调整
        size_t global_work_size = ((num_points + local_work_size - 1) / local_work_size) * local_work_size;
        
        if (local_work_size > global_work_size) local_work_size = global_work_size;
        
        err = clEnqueueNDRangeKernel(command_queue_, kernel_, 1, NULL,
                                    &global_work_size, &local_work_size,
                                    0, NULL, NULL);
        check_opencl_error(err, "Failed to enqueue kernel");
        
        // 阻塞式读取结果
        // std::vector<float> output_data(num_points * 3);
        // err = clEnqueueReadBuffer(command_queue_, d_output_, CL_TRUE, 0,
        //                          output_data.size() * sizeof(float), output_data.data(),
        //                          0, NULL, NULL);
        // check_opencl_error(err, "Failed to read output buffer");
        //非阻塞式读取
        cl_event read_event;
        std::vector<float> output_data(num_points * 4);
        for(int i=0;i<num_points;++i)output_data[i+3]=0.0;
         err = clEnqueueReadBuffer(command_queue_, d_output_, CL_FALSE, 0,
                                 output_data.size() * sizeof(float), output_data.data(),
                                 0, NULL, &read_event);
        // 阻塞等待读取完成
            clWaitForEvents(1, &read_event);
            clReleaseEvent(read_event);  // 释放事件资源                         
        check_opencl_error(err, "Failed to read output buffer");
        ///////////////////////////////////////////////////////////////////
        // 更新点云数据
        int count_del=0;
        int count_add=0;
         for (int i = 0; i < num_points; i++) {
            if(ANGLE_FILTER_ENABLE==1)
            {
                if(i==0)
                {
                cloud->points[i].x = output_data[i * 4];
                cloud->points[i].y = output_data[i * 4 + 1];
                cloud->points[i].z = output_data[i * 4 + 2];
                }
                else{
                        if(output_data[i * 4 + 3]<-0.01)
                        {
                        cloud->points[i].x = 0;
                        cloud->points[i].y = 0;
                        cloud->points[i].z = 0;  count_del++;
                        }
                        else
                        {
                        cloud->points[i].x = output_data[i * 4];
                        cloud->points[i].y = output_data[i * 4 + 1];
                        cloud->points[i].z = output_data[i * 4 + 2]; count_add++;
                        }
                }
            }
            else
            {
                 cloud->points[i].x = output_data[i * 4];
                 cloud->points[i].y = output_data[i * 4 + 1];
                 cloud->points[i].z = output_data[i * 4 + 2];
            }
        }
        std::cout<<"conun_del="<<count_del<<"  count_add="<<count_add<<std::endl;
    }
    // 矩阵乘法
    void matrix_multiply(const float* a, const float* b, float* result)
    {
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                result[i * 3 + j] = 0;
                for (int k = 0; k < 3; k++) {
                    result[i * 3 + j] += a[i * 3 + k] * b[k * 3 + j];
                }
            }
        }
    }
    // 计算旋转矩阵 (Z-Y-X 欧拉角顺序)
    void calculate_rotation_matrix(float alpha, float beta, float omega, float* rotation)
    {
        // 绕X轴旋转 (alpha)
        float Rx[9] = {
            1, 0, 0,
            0, cos(alpha), -sin(alpha),
            0, sin(alpha), cos(alpha)
        };
        
        // 绕Y轴旋转 (beta)
        float Ry[9] = {
            cos(beta), 0, sin(beta),
            0, 1, 0,
            -sin(beta), 0, cos(beta)
        };
        
        // 绕Z轴旋转 (omega)
        float Rz[9] = {
            cos(omega), -sin(omega), 0,
            sin(omega), cos(omega), 0,
            0, 0, 1
        };
        
        // 组合旋转矩阵 R = Rz * Ry * Rx
        matrix_multiply(Rz, Ry, rotation);
        float temp[9];
        matrix_multiply(rotation, Rx, temp);
        memcpy(rotation, temp, 9 * sizeof(float));
    }
void release_opencl(void)
{
        clReleaseKernel(kernel_);
        clReleaseProgram(program_);
        clReleaseMemObject(d_input_);
        clReleaseMemObject(d_output_);
        clReleaseMemObject(d_rotation_);
        clReleaseMemObject(d_translation_);
        clReleaseCommandQueue(command_queue_);
        clReleaseContext(context_);

}   